

#include "log/log2.h"
#include "BSP_Sleep.h"

#include <phApp_Init.h>
#include <Reader/Reader.h>
#include <Reader/ReaderISO14443.h>
#include "CardCommand/CardCommand.h"
#include "Utility/Des.h"

#include "config/zkcfg.h"
#include "reader_cfg.h"
/*===================================================================*/
#define ZKCFG_AID		"zkcfg"
#define ZKCFG_FID_APP	0x0001
#define ZKCFG_FID_KEY	0x0010
#define ZKCFG_FID_CFG	0x0011

#define ZKCFG_MAC_SIZE		4

#define ZKCFG_FILE_DATA_SIZE		128

#if 0
enum {
	ZKCFG_S_OK,
	ZKCFG_S_AID,	
	ZKCFG_S_InAuth,
	ZKCFG_S_ExAuth,
	ZKCFG_S_SELECT_FILE,
	ZKCFG_S_READ,
	ZKCFG_S_APP,
	ZKCFG_S_KDATA,
	ZKCFG_S_CFG,
	ZKCFG_S_MAC,
	ZKCFG_S_LEN,
};
#endif 

enum {
	ZKCFG_K_InAuth,
	ZKCFG_K_ExAuth,
	ZKCFG_K_AppEnc,
	ZKCFG_K_AppMac,
	ZKCFG_K_DataEnc,
	ZKCFG_K_DataMac,
};

typedef struct {
	uint16_t length;
	uint8_t data[ZKCFG_FILE_DATA_SIZE];
}zkcfg_file_t;



static uint8_t zkcfg_rkey[16] = 
	{
//		0xFF, 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
//		0xFF, 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
		0x7A, 0x72, 0x2E, 0x2F, 0x72, 0x5B, 0x3B, 0x5D, 
		0x2D, 0x30, 0x61, 0x64, 0x69, 0x6F, 0x69, 0x70

	};
#define OUTPUT_BITS (0x7fffffffU)
#define MULTIPLIER (1103515245U)
#define INCREMENT (12345U)

static unsigned long srand_seed = 1;
static unsigned char initialized = 0;

unsigned int xTaskGetTickCount( void );

static void zk_srand(unsigned int s)
{
	srand_seed = s;
}

static int zk_rand(void)
{
	srand_seed = (MULTIPLIER * srand_seed + INCREMENT) & OUTPUT_BITS;

	return srand_seed;
}

/* attempt to mimic openssl's RAND_bytes and create a cryptographic random
 * number generator.
 */
void rand_bytes(unsigned char *buf, int num) {
  int i;
  
  if (!initialized) {
    initialized = 1;
	zk_srand(xTaskGetTickCount());
  }
  
  for (i=0; i<num; ++i) {
    buf[i] = (unsigned char) zk_rand();
  }
}

static void des3_enc_cbc(uint8_t iv[8], const uint8_t * input, uint8_t * output, uint32_t len, const uint8_t keys[16])
{
	mbedtls_des3_context ctx3;
	mbedtls_des3_init(&ctx3);
	mbedtls_des3_set2key_enc(&ctx3, keys);
	mbedtls_des3_crypt_cbc( &ctx3, MBEDTLS_DES_ENCRYPT, len, iv, input, output );
}


static void des3_dec_cbc(uint8_t iv[8], const uint8_t * input, uint8_t * output, uint32_t len, const uint8_t keys[16])
{
	mbedtls_des3_context ctx3;

	mbedtls_des3_init(&ctx3);
	mbedtls_des3_set2key_dec(&ctx3, keys);
	mbedtls_des3_crypt_cbc( &ctx3, MBEDTLS_DES_DECRYPT, len, iv, input, output );
}

static void des_enc_cbc(uint8_t iv[8],const uint8_t * input, uint8_t * output, uint32_t len, const uint8_t keys[8]){
	mbedtls_des_context ctx;

	mbedtls_des_init( &ctx );
	mbedtls_des_setkey_enc(&ctx, keys);
	mbedtls_des_crypt_cbc( &ctx, MBEDTLS_DES_ENCRYPT, len, iv, input, output );
}

static void des_enc(const uint8_t input[8], uint8_t output[8], const uint8_t keys[8]){
	mbedtls_des_context ctx;

	mbedtls_des_init( &ctx );
	mbedtls_des_setkey_enc(&ctx, keys);
	mbedtls_des_crypt_ecb( &ctx, input, output );
}

static void des_dec(const uint8_t input[8], uint8_t output[8], const uint8_t keys[8]){
	mbedtls_des_context ctx;

	mbedtls_des_init( &ctx );
	mbedtls_des_setkey_dec(&ctx, keys);
	mbedtls_des_crypt_ecb( &ctx, input, output );
}

static void des3_enc(const uint8_t input[8], uint8_t  output[8], const uint8_t keys[16]){
	mbedtls_des3_context ctx3;

	mbedtls_des3_init(&ctx3);
	mbedtls_des3_set2key_enc(&ctx3, keys);
	mbedtls_des3_crypt_ecb( &ctx3, input, output );
}

static void reader_zkcfg_enc(const uint8_t* input, uint8_t* output, uint32_t len, const uint8_t keys[16]){
	uint8_t iv[8] = {0};
	des3_enc_cbc(iv, input, output, len, keys);
}	

static void reader_zkcfg_dec(const uint8_t* input, uint8_t* output, uint32_t len, const uint8_t keys[16]){
	uint8_t iv[8] = {0};
	des3_dec_cbc(iv, input, output, len, keys);
}

static uint32_t reader_zkcfg_mac(const uint8_t* input, uint32_t length, const uint8_t keys[16]){
	uint8_t iv[8] = {0};
	uint8_t output[8];
	uint32_t mac;
    int i;
	mbedtls_des_context ctx;
	
	LOG2_BUF_DBG("mac msg", input, length);
	LOG2_BUF_DBG("mac key", keys, 16);
	
    if( length % 8 ){
		LOG2_ERR("length = %d", length);
		return 0;
	}
	
	mbedtls_des_init( &ctx );
	mbedtls_des_setkey_enc(&ctx, keys);

	while( length > 0 )
	{
		for( i = 0; i < 8; i++ )
			output[i] = (unsigned char)( input[i] ^ iv[i] );
	
		mbedtls_des_crypt_ecb(& ctx, output, output );
		memcpy( iv, output, 8 );
	
		input  += 8;
		length -= 8;
	}
	
	des_dec(output, output, &keys[8]);
	des_enc(output, output, keys);
	memcpy(&mac, output, 4);
	return mac;
}

//分散密钥
static void reader_zkcfg_key_div(const uint8_t div[16], uint8_t output[16]){
	reader_zkcfg_enc(div, output, 16, zkcfg_rkey);
}

static uint8_t reader_zkcfg_pkcs7_pad(uint32_t dlen){
	uint32_t block_size = 8;
	uint8_t remain = dlen % block_size;
	if(remain != 0){
		return block_size - remain;
	}
	return block_size;
}

static void reader_copy_inv(uint8_t* output, const uint8_t* input, uint32_t len){
	for(uint32_t i = 0; i < len; i++){
		output[i] = ~ input[i];
	}
}

static void reader_zkcfg_get_uid_div(uint8_t type, const uint8_t uid[4], uint8_t div[16]){

	switch(type){
	case ZKCFG_K_InAuth:{
		memcpy(div, uid, 4);
		reader_copy_inv(div + 4, uid, 4);
	}
	break;
	case ZKCFG_K_ExAuth:{
		reader_copy_inv(div, uid, 4);
		memcpy(div + 4, uid, 4);
	}
	break;
	case ZKCFG_K_AppEnc:{
		reader_copy_inv(div, uid, 4);
		reader_copy_inv(div + 4, uid, 4);
	}
	break;
	default:{
		memcpy(div, uid, 4);
		memcpy(div + 4, uid, 4);
	}
	break;
	}

	memset(div + 8, 0x08, 8);
	
}

static int reader_zkcfg_inter_auth(const uint8_t uid[4]){
	int ret;
	uint8_t keys[16];
	uint8_t div[16];
	uint8_t rand_buf[8] = {0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x77};
	uint8_t resp [16];
	uint16_t rlen = 0;
	uint8_t keyId = 0;
	//内部认证
	LOG2_INF("%s", __FUNCTION__);
	
	reader_zkcfg_get_uid_div(ZKCFG_K_InAuth, uid, div);
	reader_zkcfg_key_div(div, keys);
	rand_bytes(rand_buf, 8);
	
	LOG2_BUF_DBG("div", div, 16);
	LOG2_BUF_DBG("rand", rand_buf, 8);
	
	ret = Card_RawInternalAuth(0, keyId, rand_buf, 8, resp, &rlen);
	if(ret){
		LOG2_ERR("Card_RawInternalAuth fail: %d", ret);
		return ret;
	}
	
	if(rlen != 10 || resp[rlen - 2] != 0x90 || resp[rlen - 1] != 0x00){
		LOG2_BUF_ERR("internal auth resp", resp, rlen);
		return ZKCFG_S_InAuth;
	}
	
	LOG2_BUF_DBG("resp", resp, rlen);
	LOG2_BUF_DBG("keys", keys, 16);
	des3_enc(rand_buf, rand_buf, keys);
	LOG2_BUF_DBG("enc rand", rand_buf, 8);
	if(memcmp(rand_buf, resp, 8)){
		LOG2_BUF_ERR("internal auth resp", resp, rlen);
		return ZKCFG_S_InAuth;
	}
	return ZKCFG_S_OK;
}

static int reader_zkcfg_extern_auth(const uint8_t uid[4]){
	int ret;
	uint8_t keys[16];
	uint8_t div[16];
	uint8_t rand_buf[8];
	uint8_t keyId = 0;
	uint8_t card = 0;
	LOG2_INF("%s", __FUNCTION__);
	//外部认证
	reader_zkcfg_get_uid_div(ZKCFG_K_ExAuth, uid, div);
	reader_zkcfg_key_div(div, keys);
	
	ret = Card_GetChallenge(card, 8, rand_buf);
	if(ret){
		LOG2_ERR("Card_GetChallenge fail: %d", ret);
		return ret;
	}
	LOG2_BUF_DBG("rand", rand_buf, 8);
	reader_zkcfg_enc(rand_buf, rand_buf, 8, keys);
	ret = Card_RawExternalAuth(card, keyId, rand_buf, 8);
	if(ret){
		LOG2_ERR("Card_RawExternalAuth fail: %d", ret);
		return ret;
	}

	return ZKCFG_S_OK;
}

static int reader_zkcfg_read_file_raw(uint16_t fid, zkcfg_file_t* fdata, uint16_t max_length){
	int ret;
	uint8_t card = 0;
	
	ret = Card_SelectFileById(card, fid);
	if(ret){
		LOG2_ERR("Card_SelectFileById(%04X) fail: %d", fid, ret);
		return ZKCFG_S_SELECT_FILE;
	}
	
	ret = Card_ReadBinary0(card, 0, (uint8_t*)&fdata->length , sizeof(fdata->length));
	if(ret){
		LOG2_ERR("get length fail: %d", ret);
		return ZKCFG_S_READ;
	}
	if(fdata->length > max_length){
		LOG2_ERR("length = %d, max_length = %d", fdata->length, max_length);
		return ZKCFG_S_LEN;
	}
	ret = Card_ReadBinary0(card, sizeof(fdata->length), fdata->data, fdata->length);
	if(ret){
		LOG2_ERR("get data fail: %d", ret);
		return ZKCFG_S_READ;
	}
	
	return ZKCFG_S_OK;
}

static uint32_t reader_zkcfg_get_file_mac(zkcfg_file_t* file){
	uint32_t mac;
	uint8_t pad = file->data[file->length-1];
	uint8_t* pmac = file->data + file->length - pad - ZKCFG_MAC_SIZE;
	
	memcpy(&mac, pmac, ZKCFG_MAC_SIZE);
	
	return mac;
}

static int reader_zkcfg_read_file(uint16_t fid, uint8_t keys_enc[16], uint8_t keys_mac[16], zkcfg_file_t* file){
	uint32_t mac;
	uint32_t fmac;
	int ret;
	ret = reader_zkcfg_read_file_raw(fid, file, ZKCFG_FILE_DATA_SIZE);
	if(ret){
		LOG2_ERR("read file fail: %d", ret);
		return ret;
	}
	//解密
	reader_zkcfg_dec(file->data, file->data, file->length, keys_enc);
	fmac = reader_zkcfg_get_file_mac(file);
	uint8_t pad = file->data[file->length-1];
	uint8_t len = file->length - pad;
	uint8_t dlen = len - ZKCFG_MAC_SIZE;
	//校验MAC
	pad = reader_zkcfg_pkcs7_pad(dlen);
	memset(file->data + dlen, pad, pad);
	len = dlen + pad;
	mac = reader_zkcfg_mac(file->data, len, keys_mac);
	if(mac != fmac){
		LOG2_ERR("mac: %08X, fmac: %08X", mac, fmac);
		return ZKCFG_S_MAC;
	}
	file->length = dlen;
	return ZKCFG_S_OK;
}

static void reader_zkcfg_div_app_key(const uint8_t uid[4], uint8_t keys_enc[16], uint8_t keys_mac[16]){
	uint8_t div[16];

	reader_zkcfg_get_uid_div(ZKCFG_K_AppEnc, uid, div);
	reader_zkcfg_key_div(div, keys_enc);
	LOG2_BUF_DBG("enc div", div, 16);
	LOG2_BUF_DBG("keys_enc", keys_enc, 16);
	reader_zkcfg_get_uid_div(ZKCFG_K_AppMac, uid, div);
	reader_zkcfg_key_div(div, keys_mac);
	LOG2_BUF_DBG("mac div", div, 16);
	LOG2_BUF_DBG("keys_mac", keys_mac, 16);
}


#if 0
static int reader_zkcfg_read_app(uint8_t uid[4], zkcfg_app_t* app){
	int ret;
	uint8_t keys_enc[16];
	uint8_t keys_mac[16];
	uint8_t div[16];
	uint32_t mac;
	uint32_t fmac;
	uint8_t buf[64];
	uint8_t card = 0;
	
	LOG2_INF("%s", __FUNCTION__);
	
	zkcfg_file_t* file = (zkcfg_file_t*)buf;
	reader_zkcfg_get_uid_div(ZKCFG_K_AppEnc, uid, div);
	reader_zkcfg_key_div(div, keys_enc);
	LOG2_BUF_DBG("enc div", div, 16);
	LOG2_BUF_DBG("keys_enc", keys_enc, 16);
	reader_zkcfg_get_uid_div(ZKCFG_K_AppMac, uid, div);
	reader_zkcfg_key_div(div, keys_mac);
	LOG2_BUF_DBG("mac div", div, 16);
	LOG2_BUF_DBG("keys_mac", keys_mac, 16);
	
	ret = reader_zkcfg_read_file(ZKCFG_FID_APP, keys_enc, keys_mac, file, sizeof(buf)-2);
	if(ret){
		LOG2_ERR("read file fail: %d", ret);
		return ret;
	}

	memcpy(app, file->data, file->length);
	return ZKCFG_S_OK;
}
#endif 


static void reader_zkcfg_key_data_dbg(const zkcfg_key_data_t* kdata){
	LOG2_PRINT("\r\n key data: \n");
	LOG2_PRINT("\tkid: %d\n", kdata->kid);
	LOG2_PRINT("\ttype: %d\n", kdata->type);
	LOG2_PRINT("\tlength: %d\n", kdata->length);
	LOG2_PRINT("\tkeys:");
	LOG2_DUMP_HEX(kdata->key, kdata->length);
	LOG2_PRINT("\r\n");
}

void reader_zkcfg_key_data_debug(const zkcfg_key_data_t* kdata, uint16_t count){
	for(int i = 0; i < count; i++){
		reader_zkcfg_key_data_dbg(&kdata[i]);
	}
}

static int reader_zkcfg_set_key_datas(zkcfg_file_t* file){
	if( file->length < sizeof(zkcfg_keys_t)){
		LOG2_ERR("file->length: %d < sizeof(zkcfg_keys_t): %d", file->length, sizeof(zkcfg_keys_t));
		return -1;
	}
	zkcfg_keys_t* keys = (zkcfg_keys_t*)file->data;
	int ret = zkcfg_write_keys(keys);
	if(!ret){
		reader.keys = *keys;
	}
	return ret;
}

static void reader_zkcfg_keyset_dbg(const zkcfg_keyset_t* ks){
	LOG2_PRINT("\r\n key set: \n");
	LOG2_PRINT("\taid: %06X\n", ks->aid);
	LOG2_PRINT("\tapp: %d\n", ks->app);
	LOG2_PRINT("\tkey_id: %d\n", ks->key_id);
	LOG2_PRINT("\tkey_type %d\n", ks->key_type);
	LOG2_PRINT("\tkey_num: %d\n", ks->key_num);
	LOG2_PRINT("\tdiversified: %d\n", ks->diversified);
	LOG2_PRINT("\r\n");
}

void reader_zkcfg_keyset_debug(zkcfg_file_t* file){
	const zkcfg_keyset_t *keyset = (const zkcfg_keyset_t *)file->data;
	uint8_t count = file->length / sizeof(zkcfg_keyset_t);
	
	for(int i = 0; i < count; i++){
		reader_zkcfg_keyset_dbg(keyset+i);
	}
}


static int reader_zkcfg_set_key_cfgs(zkcfg_file_t* file){
	reader_zkcfg_keyset_debug(file);
	if(file->length < sizeof(zkcfg_cfg_t)){
		LOG2_ERR("file->length: %d < sizeof(zkcfg_cfg_t): %d", file->length, sizeof(zkcfg_cfg_t));
		return -1;
	}
	zkcfg_cfg_t* zkcfg = (zkcfg_cfg_t*)file->data;
	int ret = zkcfg_write(ZKCFG_PAGE_CFG, 0, sizeof(zkcfg_cfg_t), zkcfg);
	if(!ret){
		reader.zkcfg = *zkcfg;
	}
	return ret;
}


static void reader_zkcfg_div_data_key(const uint8_t data_div[8], uint8_t keys_enc[16], uint8_t keys_mac[16]){
	uint8_t div[16];
	
	memcpy(div, data_div, 8);
	memset(div + 8, 8, 8);

	reader_zkcfg_key_div(div, keys_enc);
	LOG2_BUF_DBG("enc div", div, 16);
	LOG2_BUF_DBG("keys_enc", keys_enc, 16);
	
	reader_copy_inv(div, data_div, 8);
	reader_zkcfg_key_div(div, keys_mac);
	LOG2_BUF_DBG("mac div", div, 16);
	LOG2_BUF_DBG("keys_mac", keys_mac, 16);
}


int reader_zkcfg_read(const uint8_t uid[4]){
	int ret;
	uint8_t card = 0;
	//1.选AID
	uint8_t aid[] = ZKCFG_AID;
	uint8_t alen = sizeof(ZKCFG_AID)-sizeof("");
	
	LOG2_INF("Card_SelectAID: %X", aid);
	if(Card_SelectAID(card, aid, alen)){
		return ZKCFG_S_AID;
	}
	//2.内部认证
	if(reader_zkcfg_inter_auth(uid)){
		return ZKCFG_S_InAuth;
	}
	//3.外部认证
	if(reader_zkcfg_extern_auth(uid)){
		return ZKCFG_S_ExAuth;
	}
	
	uint8_t keys_enc[16];
	uint8_t keys_mac[16];	

	zkcfg_file_t file_app;
	zkcfg_app_t* app = (zkcfg_app_t*)file_app.data;
	//分散app密钥
	reader_zkcfg_div_app_key(uid, keys_enc, keys_mac);
	//读取应用信息
	ret = reader_zkcfg_read_file(ZKCFG_FID_APP, keys_enc, keys_mac, &file_app);
	if(ret){
		return ZKCFG_S_APP;
	}
	//分散data密钥
	reader_zkcfg_div_data_key(app->div, keys_enc, keys_mac );
	zkcfg_file_t file_key;
	file_key.length = 0;
	//读取密钥数据
	ret = reader_zkcfg_read_file(ZKCFG_FID_KEY, keys_enc , keys_mac , &file_key);
	if(ret && ret != ZKCFG_S_SELECT_FILE){
		return ZKCFG_S_KDATA;
	}

	//读取配置数据
	zkcfg_file_t file_cfg;
	file_cfg.length = 0;
	ret = reader_zkcfg_read_file(ZKCFG_FID_CFG, keys_enc , keys_mac , &file_cfg);
	if(ret && ret != ZKCFG_S_SELECT_FILE){
		return ZKCFG_S_CFG;
	}
	//保存密钥/配置
	if(file_key.length != 0){
		ret = reader_zkcfg_set_key_datas(&file_key);
		if(ret){
			return ZKCFG_S_CFG;
		}
	}
	
	if(file_cfg.length != 0){
		ret = reader_zkcfg_set_key_cfgs(&file_cfg);
		if(ret){
			return ZKCFG_S_CFG;
		}
	}
	return ZKCFG_S_OK;
}

void reader_zkcfg_init(void){
	int ret = zkcfg_read(ZKCFG_PAGE_MANU, 0, 16, zkcfg_rkey);
	if(ret){
		LOG2_WRN("read rkey fail, use default: ");
		LOG2_DUMP_HEX(zkcfg_rkey, 16);
	}
}


